package com.allesblinkt.leicasdream;

import mathematik.Vector3f;

public class PositionHistory {

	private int _size;
	private int _pointer = 0;
	
	private Vector3f[] _positions;
	public int length;
	
	public PositionHistory(int theSize){
		_size = theSize;
		
		_positions = new Vector3f[_size];
		length = theSize;
		
	}
	
	public void store(Vector3f thePosition) {
		
		if(_positions[_pointer] == null){
			_positions[_pointer] = new Vector3f(thePosition);
		} else {
			_positions[_pointer].set(thePosition); // Reuse vector
		}
		
		_pointer = (_pointer +1 ) % _size;
	}
	
	
	public Vector3f get(int theIndex){
		
		if(theIndex < _size && theIndex >= 0){
			
		
			int wrappedIndex = (_pointer + theIndex) % _size;
			
			
			return _positions[wrappedIndex];
			
		}
		
		return null;
	}
	
	public Vector3f[] getSortedArray(int theMax){
		int maxCount = Math.min(theMax, _size);

		Vector3f[] returnArray = new Vector3f[maxCount];
		
		
		for(int i = 0; i<maxCount; i++){
			
			Vector3f vector = get(i);
			
			if(vector == null) vector = new Vector3f(0,0,0);
			returnArray[i] = new Vector3f(vector);
			
		}
		
		return returnArray;
	}
}
